import numpy as np

def F2_InputAugmentMatrix_SS_U(A, B, Q, R, S, xd):
    # 计算系统矩阵维度，n
    n = A.shape[0]
    
    # 计算输入矩阵维度，p
    p = B.shape[1]

    # 构建增广矩阵Ca，参考式（4.5.17）
    Ca = np.hstack((np.eye(n), -np.eye(n)))

    # 构建增广矩阵Aa，参考式（4.5.16b）
    Aa = np.block([[A, np.eye(n) - A],
                   [np.zeros((n, n)), np.eye(n)]])

    # 构建增广矩阵Ba，参考式（4.5.16b）
    Ba = np.vstack((B, np.zeros((n, p))))

    # 构建增广矩阵Qa，参考式（4.5.18）
    Qa = Ca.T @ Q @ Ca

    # 构建增广矩阵Sa，参考式（4.5.18）
    Sa = Ca.T @ S @ Ca

    # R 矩阵不变
    R = R

    # 计算稳态控制输入ud
    # 相当于 ud = B \ ((I - A) * xd) in MATLAB
    ud = np.linalg.pinv(B) @ (np.eye(n) - A) @ xd

    return Aa, Ba, Qa, Sa, R, ud